
#include <spu_intrinsics.h>
#include <spu_mfcio.h>
#include "cmatrixprod.h"

/* Variables Globales */

t_complex bloc_a[BLOC_SIZE*BLOC_SIZE] __attribute__((aligned(16)));
t_complex bloc_b[BLOC_SIZE*BLOC_SIZE] __attribute__((aligned(16)));
t_complex bloc_c[BLOC_SIZE*BLOC_SIZE] __attribute__((aligned(16)));
t_complex buff_a[BLOC_SIZE*BLOC_SIZE] __attribute__((aligned(16)));
t_complex buff_b[BLOC_SIZE*BLOC_SIZE] __attribute__((aligned(16)));

abs_params_t abs_params __attribute__((aligned(16)));

int step,spe_num,tid = 0, rid = 0, tag = 1;
uint32_t tag_mask=1<<1;
unsigned int res;
uint64_t next_addr;


/* Fonction d'affichage */
void affiche(t_complex a[], int nlig, int ncol, char *s){
  int i,j;
  printf("\n__SPE\n%s=[\n",s);
  for (i=0; i<nlig;i++) {
    for (j=0;j<ncol;j++) 
      printf("%f + %f*i  ",a[i*ncol+j].r,a[i*ncol+j].i);
    printf(";\n"); 
  }   
 printf("];\n");
}

void calculation_fma(int M, int N, int P, t_complex *A, 
		 t_complex *B, t_complex *C){
  int i,j,k;
  t_complex som;
  
  vector float va,vb,vc,vd,vbd,vbc,vi,vr,vres1,vres2,vres;
  vector float *v1=(vector float *)A;
  vector float *v2=(vector float *)B;  
  vector float *v3=(vector float *)C;  

  vector float zero= (vector float){0,0,0,0};
  
  /*pour recuperer toutes les parties reelles*/
  vector unsigned char m1=(vector unsigned char)
    {0,1,2,3,8,9,10,11,16,17,18,19,24,25,26,27};

  /*pour recuperer toutes les parties imaginaires*/
  vector unsigned char m2=(vector unsigned char)
    {4,5,6,7,12,13,14,15,20,21,22,23,28,29,30,31};

  vector unsigned char vvv=(vector unsigned char)
    {0,1,2,3,16,17,18,19,4,5,6,7,20,21,22,23};

  vector unsigned char xxx=(vector unsigned char)
    {8,9,10,11,24,25,26,27,12,13,14,15,28,29,30,31};

  // ligne
  for(i=0; i<M/4; i++){
    // colonne
    for(j=0; j<N/4; j++){
      // boucle de sommes
      for(k=0; k<P/4; k++){

	/*tous les reels*/
	va=spu_shuffle(v1[k*2 + i*M], v1[k*2+1 + i*M], m1);
	vc=spu_shuffle(v2[k*N + j], v2[(k+1)*N + j], m1);
	
	/*tous les imaginaires*/
	vb=spu_shuffle(v1[k*2 + i*M], v1[k*2+1 + i*M], m2);
	vd=spu_shuffle(v2[k*N + j], v2[(k+1)*N + j], m2);
	
	vbd=spu_nmsub(vb,vd,zero);
	vbc=spu_madd(vb,vc,zero);
	vi=spu_madd(va,vd,vbc);
	vr=spu_madd(va,vc,vbd);
	
	vres1=spu_shuffle(vr,vi,vvv);
	vres2=spu_shuffle(vr,vi,xxx);

	v3[i*P + j]=spu_add(v3[i*P+j],vres1);
	v3[i*P + j]=spu_add(v3[i*P+j],vres2);

	/* v3[i*P + j] = (vector float){vres1[0]+vres1[2],vres1[1]+vres1[3],vres2[0]+vres2[2],vres2[1]+vres2[3]}; */
	
      }
    }
  }

/* Fonction de calculation de multiplication complexe */
void calculation(int M, int N, int P, t_complex *A, 
		 t_complex *B, t_complex *C){
  int i,j,k;
  t_complex som;
  for(i=0;i<M;i++){
    for (j=0;j<N;j++){
      som=C[i*N+j];
      for (k=0;k<P;k++) {
	som.r = som.r + A[i*P+k].r*B[k*N+j].r-A[i*P+k].i*B[k*N+j].i;
	som.i = som.i + A[i*P+k].r*B[k*N+j].i+A[i*P+k].i*B[k*N+j].r;
      }
      C[i*N+j]=som;
    }
  }
}

void load_bloc_a(int n){

  int i;
  for (i=0; i<BLOC_SIZE; i++){
    mfc_get(bloc_a + i * BLOC_SIZE,
	    abs_params.in_matrix_A + (abs_params.size_M*i + BLOC_SIZE * spe_num + abs_params.size_M * BLOC_SIZE * n) * 
	    sizeof(t_complex),
	    BLOC_SIZE*sizeof(t_complex),
	    tag, tid, rid);
    mfc_write_tag_mask(tag_mask);
    res = mfc_read_tag_status_all();
  }
}

void load_next_bloc_a(int n, int m){

  int i;
  for (i=0; i<BLOC_SIZE; i++){
    mfc_get(bloc_a + i * BLOC_SIZE,
	    abs_params.in_matrix_A + 
	    ( (abs_params.size_M*i + BLOC_SIZE * spe_num + abs_params.size_M * BLOC_SIZE * n + BLOC_SIZE * m) % 
	      (abs_params.size_M * abs_params.size_M) ) * 
	    sizeof(t_complex),
	    BLOC_SIZE*sizeof(t_complex),
	    tag, tid, rid);
    mfc_write_tag_mask(tag_mask);
    res = mfc_read_tag_status_all();
  }
}

void load_bloc_b(int n){
  
  int i;
  for (i=0; i<BLOC_SIZE; i++){
    mfc_get(bloc_b + i*BLOC_SIZE,
	    abs_params.in_matrix_B  + 
	    ( (BLOC_SIZE * spe_num + abs_params.size_N*i + abs_params.size_N * BLOC_SIZE * n + abs_params.size_N * BLOC_SIZE * spe_num) % 
	      (abs_params.size_N * abs_params.size_N) )
	    * sizeof(t_complex),
	    BLOC_SIZE*sizeof(t_complex),
	    tag, tid, rid);
    mfc_write_tag_mask(tag_mask);
    res = mfc_read_tag_status_all();
  }
}

void load_bloc_c(int n){

  int i;
  for (i=0; i<BLOC_SIZE; i++){
    mfc_get(bloc_c + i*BLOC_SIZE,
	    abs_params.out_matrix_C + (abs_params.size_P*i + BLOC_SIZE * spe_num + abs_params.size_P * BLOC_SIZE * n) * 
	    sizeof(t_complex),
	    BLOC_SIZE*sizeof(t_complex),
	    tag, tid, rid);
    mfc_write_tag_mask(tag_mask);
    res = mfc_read_tag_status_all();
  }
}

void store_bloc_c(int n){

  int i;
  for (i=0; i<BLOC_SIZE; i++){
    mfc_put(bloc_c + i*BLOC_SIZE,
	    abs_params.out_matrix_C + (abs_params.size_P * BLOC_SIZE * n + BLOC_SIZE * spe_num + abs_params.size_P*i) * 
	    sizeof(t_complex),
	    BLOC_SIZE*sizeof(t_complex),
	    tag, tid, rid);
    mfc_write_tag_mask(tag_mask);
    res = mfc_read_tag_status_all();
  }

}

void get_addr(){
  uint32_t adrl,adrh;
  /*  SEND BUFF ADDR  */
  while (spu_stat_out_mbox() == 0);
  spu_write_out_mbox((uint32_t) buff_a); // TODO ??
  /*  RECV BUFF ADDR  */
  while (spu_stat_in_mbox() == 0);
  adrh = spu_read_in_mbox();
  while (spu_stat_in_mbox() == 0);
  adrl = spu_read_in_mbox();
  next_addr = mfc_hl2ea(adrh,adrl);
}


int main(unsigned long long spe, unsigned long long argp, unsigned long long envp)
{

  int i,j,sig;

  /* SEND & RECV ADDRESSES */
  get_addr();
  
  /* DMA Transfer 1 : GET input/output parameters */
  mfc_get(&abs_params, argp, sizeof(abs_params_t), tag, tid, rid);
  mfc_write_tag_mask(tag_mask);
  res = mfc_read_tag_status_all();

  spe_num = abs_params.spe_num;


  /***************************
   *      MAIN ALGORITHM     *
   ***************************/
  
  for (i=0; i<NUM_SPE; i++){

    if (spe_num == 1){
      return 0;
    }

    /* load from A(i,0) */
    load_bloc_a(i);

    load_bloc_c(i);
    
    for (j=0; j<NUM_SPE; j++){

      /* load from B(j+spe_num,spe_num) */
      load_bloc_b(j);
      
      /* bloc_c() = bloc_c() + bloc_a * bloc_b */
      calculation(BLOC_SIZE, BLOC_SIZE, BLOC_SIZE, bloc_a, bloc_b, bloc_c);
      
      load_next_bloc_a(i,j);


	// VERSION DE CHARGEMENT ENTRE SPEs SYNCHRONISE //

      /* /\* read from SPEi+1 into buff *\/  // TODO : PUT buff_a INTO 64 bit???  WHAT SIZE FOR PUT ??? */
/*       mfc_put(bloc_a, next_addr, BLOC_SIZE * BLOC_SIZE * sizeof(t_complex), tag, tid, rid); */
/*       //mfc_put(bloc_a, next_addr, BLOC_SIZE * sizeof(t_complex), tag, tid, rid); */
	      
/*       /\* Sync, Copy buff into bloc_a *\/ */

/*       /\* send signal *\/ */
/*       while(spu_stat_out_mbox() == 0); */
/*       spu_write_out_mbox((uint32_t)READY); */


/*       /\* wait for signal *\/ */
/*       while(spu_stat_in_mbox() == 0); */
/*       sig = spu_read_in_mbox(); */
/*       if (sig != GO){ */
/* 	perror("__SPE didn't receive a GO"); */
/* 	exit(1); */
/*       } */
/*       /\* copy received bloc from buffer *\/ */
/*       memcpy(bloc_a,buff_a,BLOC_SIZE*BLOC_SIZE*sizeof(t_complex)); */

/*       /\* send signal *\/ */
/*       while(spu_stat_out_mbox() == 0); */
/*       spu_write_out_mbox((uint32_t)READY); */


/*       /\* wait for signal *\/ */
/*       while(spu_stat_in_mbox() == 0); */
/*       sig = spu_read_in_mbox(); */
/*       if (sig != GO){ */
/* 	perror("__SPE didn't receive a GO"); */
/* 	exit(1); */
/*       } */

    }

    store_bloc_c(i);

  }

  return 0;
}

